
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;


public class OdinTest {
	
	public static void main(String[] args) throws Exception {
		
		LCD.drawString("Odin Test", 0, 0);
		
		Motor.A.setSpeed(300);
		Motor.B.setSpeed(300);
		Motor.C.setSpeed(300);
		
		while(true) {
		
			upDown(false);
			Thread.sleep(1000);
			turn(90);
			Thread.sleep(1000);
			upDown(true);
			goForward(33);
			Button.waitForPress();
		}
		
	}
	
	private static void upDown(boolean up) {
		int angle = 340;
		
		if (up)
			angle = 0;
		
		Motor.C.rotateTo(angle, false);
	}
	
	private static void turn(int angle) {
		
		//since gear ratio is 1:7
		Motor.A.rotate(angle*7, false);
	}
	
	private static void goForward(int distance) {
		
		int angle = (int) Math.round(360 * (distance / (2 * Math.PI *5.5)));
		LCD.drawString("..." + angle + "...", 0, 1);
		Motor.B.rotate(angle, false);
		
	}

}
